#include "paths_routing.h"


Paths_Routing::Paths_Routing()
    : last_vehicle_index(0)
{
    num_next = ceil((prp.MaxT * prp.target_speed) / 0.3) + 12; 
    //消息发布
    vehicle_outline_pub_ = nh.advertise<visualization_msgs::Marker>("/vehicle_outline", 10);
    local_paths_pub_ = nh.advertise<nav_msgs::Path>("/xyl/local_paths", 10);       // 发布局部轨迹
    local_paths_a = nh.advertise<geometry_msgs::PoseArray>("/xyl/local_acceleration", 10);
    local_paths_v = nh.advertise<geometry_msgs::PoseArray>("/xyl/local_velocity", 10);
    local_paths_t = nh.advertise<geometry_msgs::PoseArray>("/xyl/local_theta", 10);
    local_paths_k = nh.advertise<geometry_msgs::PoseArray>("/xyl/local_kappas", 10);
    Start_Dynamic = nh.advertise<std_msgs::String>("/xyl/planning/Start_Dynamic", 10);           // 发布局部轨迹成功生存的信号
    //消息订阅
    global_smooth_paths_ = nh.subscribe("/xyl/refer_Point", 10, &Paths_Routing::Global_Smooth_Paths, this);
    odom_ = nh.subscribe("/odom", 10, &Paths_Routing::Odom, this);
    goal_pose_ = nh.subscribe("/move_base_simple/goal", 10, &Paths_Routing::GoalPose, this);
    scan_ = nh.subscribe("/scan", 1000, &Paths_Routing::Scan, this);

    std::string aa = "0";
    start_dynamic.data = aa.c_str();
    Start_Dynamic.publish(start_dynamic);

    segment_intercept_thread_ = std::make_unique<std::thread>(&Paths_Routing::SegmentInterceptThread, this);
    paths_routing_thread_ = std::make_unique<std::thread>(&Paths_Routing::PathsRoutingThread,this);
}

void Paths_Routing::PathsRoutingThread(void)
{
    ros::Rate loop_rate(10);
    FrenetHyperparameters fot_hp = createFrenetHyperparameters();
    while (ros::ok())
    {
        if(is_global_s_heading_ && is_path_segment && is_paths_finish && local_finish)
        {
            std::vector<Point> obstacles_ptr = obs.getObstaclesAsPointers();
            if(messege2 == true)
            {
                ds0 = best_ds0;                      // 初始的纵向速度[m/s]
            }
            best_frenet_path = nullptr;
            std::priority_queue<std::unique_ptr<FrenetPath>, std::vector<std::unique_ptr<FrenetPath>>, CostComparator_for_Frenet> frenet_paths;
            FrenetInitialConditions fot_ic = createFrenetInitialConditions();

            fot.calc_frenet_paths(&fot_ic, &fot_hp, frenet_paths, global_paths, 
                                               PathSegment.accumulated_s, PathSegment.index, CubSpline2);
            
            // 筛选最优轨迹
            std::vector<std::unique_ptr<FrenetPath>> valid_paths;
            double half_width = prp.vehicle_width;
            double threshold_levels[] = {half_width + 0.2, half_width + 0.1, half_width + 0.05};
            int selected_path_index = 0; // 记录选中的路径索引
            double selected_threshold = 0; // 记录选中的阈值
            // 筛选最优轨迹
            while (!frenet_paths.empty())
            {
                std::unique_ptr<FrenetPath> top = std::move(const_cast<std::unique_ptr<FrenetPath>&>(frenet_paths.top()));
                frenet_paths.pop();
                if (is_collision(top, half_width + 0.05, obstacles_ptr) == false) // 通过碰撞检测
                {
                    best_frenet_path = std::move(top);  // 移动 top 到 best_frenet_path
                    break;
                }
            }
            DiscretizedTrajectory best_path = fot.FrenetPath_to_TrajectoryPoint(best_frenet_path);
            messege2 = false;
            FrenetPathsPub(best_path);
            double goal_distance = std::hypot(odom.pose.pose.position.x - goal_pose_x, odom.pose.pose.position.y - goal_pose_y);
            if (goal_distance < 0.3)
            {
                ROS_INFO("Successfully reached the target point.");
                local_finish = false;
                is_path_segment = false;
            }
        }
        ros::spinOnce();
        loop_rate.sleep();
    }
    
}

void Paths_Routing::FrenetPathsPub(const DiscretizedTrajectory& best_path)
{
    if (best_path.size() == 0){
        if(messege2 == false){
            ROS_WARN("vehical can not pass the road!");
            std::string aa = "2";
            start_dynamic.data = aa.c_str();
            Start_Dynamic.publish(start_dynamic); // 没有轨迹，发送停车信号

            traj_points_.poses.clear();
            traj_points_.header.frame_id = "map";
            traj_points_.header.stamp = ros::Time::now();
            local_paths_pub_.publish(traj_points_);
            messege1 = false; // 复位
            messege2 = true;  // 复位
        }
    }else{
        traj_points_.poses.clear();
        traj_points_.header.frame_id = "map";
        traj_points_.header.stamp = ros::Time::now();
        for (int i = 0; i < best_path.size(); i++)
        {
            geometry_msgs::PoseStamped pose_stamp;
            pose_stamp.header.frame_id = "map";
            pose_stamp.header.stamp = ros::Time::now();
            pose_stamp.pose.position.x = best_path[i].x;
            pose_stamp.pose.position.y = best_path[i].y;
            pose_stamp.pose.position.z = 0;
            traj_points_.poses.push_back(pose_stamp);
        }
        local_paths_pub_.publish(traj_points_);
        // 发布速度
        pubLocalPath_v.poses.clear();
        pubLocalPath_v.header.frame_id = "map";
        pubLocalPath_v.header.stamp = ros::Time::now();
        for (size_t i = 0; i < best_path.size(); i++)
        {
            geometry_msgs::Pose init_v;
            init_v.position.x = best_path[i].v;
            pubLocalPath_v.poses.push_back(init_v);
        }
        local_paths_v.publish(pubLocalPath_v);
        //--------------------------------发布加速度---------------------------------//
        pubLocalPath_a.poses.clear();
        pubLocalPath_a.header.frame_id = "map";
        pubLocalPath_a.header.stamp = ros::Time::now();
        for (size_t i = 0; i <  best_path.size(); i++)
        {
            geometry_msgs::Pose init_pose;
            init_pose.position.x = best_path[i].a;
            pubLocalPath_a.poses.push_back(init_pose);
        }
        local_paths_a.publish(pubLocalPath_a);
        //--------------------------------发布角度---------------------------------//
        pubLocalPath_t.poses.clear();
        pubLocalPath_t.header.frame_id = "map";
        pubLocalPath_t.header.stamp = ros::Time::now();
        for (size_t i = 0; i < best_path.size(); i++)
        {
            geometry_msgs::Pose init_pose;
            init_pose.position.x = best_path[i].theta;
            pubLocalPath_t.poses.push_back(init_pose);
        }
        local_paths_t.publish(pubLocalPath_t);
        //--------------------------------发布曲率---------------------------------//
        pubLocalPath_k.poses.clear();
        pubLocalPath_k.header.frame_id = "map";
        pubLocalPath_k.header.stamp = ros::Time::now();
        for (size_t i = 0; i <  best_path.size(); i++)
        {
            geometry_msgs::Pose init_pose;
            init_pose.position.x =  best_path[i].kappa;
            pubLocalPath_k.poses.push_back(init_pose);
        }
        local_paths_k.publish(pubLocalPath_k);

        if (messege1 == false) // 只发布一次,最开始生成局部轨迹的时候发
        {
            sleep(0.3); // 等会再开车，延时1秒
            std::string aa = "1";
            start_dynamic.data = aa.c_str();
            Start_Dynamic.publish(start_dynamic);                           // 发送局部轨迹生成信号给全局路径
            messege1 = true;
        }
        double ctf_s, ctf_d;
        cft.cartesianToFrenet(global_paths,  odom, ctf_s, ctf_d);
        if (is_update_dynamic(traj_points_, odom, prp.update_pos) == 1)
        {
            // init_lon_state = best_path[prp.update_pos].s;
            init_lon_state = ctf_s;
            ds0 = best_path[prp.update_pos].s_d;
            best_ds0 =  best_path[prp.update_pos].s_d;
            dds0 = best_path[prp.update_pos].s_dd;
            d0 = best_path[prp.update_pos].d;
            dd0 = best_path[prp.update_pos].d_d;
            ddd0 = best_path[prp.update_pos].d_dd;

            init_relative_time = 0;
        }else if (is_update_dynamic(traj_points_, odom, prp.update_pos) == 2){
            init_lon_state = ctf_s;
            int end_size = static_cast<int>(round((traj_points_.poses.size() * 2) / 3));
            ds0 = best_path[end_size].s_d;
            best_ds0 = best_path[end_size].s_d;
            dds0 = best_path[end_size].s_dd;
            d0 = ctf_d;
            dd0 = best_path[end_size].d_d;
            ddd0 = best_path[end_size].d_dd;

            init_relative_time = 0; // best_path[end_size].relative_time，动态障碍物的起始时间跟这个保持一致
        }
    }
}

int Paths_Routing::is_update_dynamic(nav_msgs::Path &trj_point_array, nav_msgs::Odometry &odom, int size)
{
    double distance = sqrt(pow(odom.pose.pose.position.x - trj_point_array.poses[size].pose.position.x, 2) +
                        pow(odom.pose.pose.position.y - trj_point_array.poses[size].pose.position.y, 2));
    if (distance < 0.5) // 接近了
    {
        return 1;
    }
    int end_size = static_cast<int>(((trj_point_array.poses.size() * 2) / 3));
    Point end_pose, odom_pose, end_next_pose;
    end_pose.x = trj_point_array.poses[end_size].pose.position.x;
    end_pose.y = trj_point_array.poses[end_size].pose.position.y;
    end_next_pose.x = trj_point_array.poses[end_size + 1].pose.position.x;
    end_next_pose.y = trj_point_array.poses[end_size + 1].pose.position.y;
    odom_pose.x = odom.pose.pose.position.x;
    odom_pose.y = odom.pose.pose.position.y;

    double angle = angleBetween(end_pose, end_next_pose, odom_pose);
    double angleInDegrees = angle * (180.0 / M_PI);
    if (angleInDegrees < 90) // 接近了
    {
        return 2;
    }
    return 0;
}

// 计算向量的点积
double Paths_Routing::dotProduct(const Point& a, const Point& b) {
    return a.x * b.x + a.y * b.y;
}
  
  // 计算向量的模长
double Paths_Routing::magnitude(const Point& a) {
    return sqrt(dotProduct(a, a));
}
  
  // 计算夹角（单位：弧度）
double Paths_Routing::angleBetween(const Point& A, const Point& B, const Point& C) {
    // 计算向量 AB 和 AC
    Point AB = {B.x - A.x, B.y - A.y};
    Point AC = {C.x - A.x, C.y - A.y};
  
    // 计算点积和模长
    double dot = dotProduct(AB, AC);
    double magAB = magnitude(AB);
    double magAC = magnitude(AC);
  
    // 计算余弦值
    double cosTheta = dot / (magAB * magAC);
  
    // 限制在 [-1, 1] 范围内，避免数值误差导致的错误
    if (cosTheta < -1.0) cosTheta = -1.0;
    if (cosTheta > 1.0) cosTheta = 1.0;
  
    // 计算并返回夹角（弧度）
    return acos(cosTheta);
}

void Paths_Routing::SegmentInterceptThread(void){
    is_path_segment = false;
    ros::Rate loop_rate(10);
    while (ros::ok())
    {
        if(!global_paths.empty() ){
            projection_point = mpp.match_and_projection_point(odom , global_paths);
            last_vehicle_index = projection_point.index;
            if(is_global_s_heading_ == false) {
                if(!mpp.ComputePathProfileWithCache(global_paths, cache)){
                    ROS_INFO("Cache computation failed.");
                } else {
                    ROS_INFO("Cache computed successfully.");
                    is_global_s_heading_ = true;
                }
            }

            if(is_global_s_heading_){
                is_path_segment = paths_segment_intercept(projection_point.index, cache);
            }
        }
        ros::spinOnce();
        loop_rate.sleep();
    }
    
}

bool Paths_Routing::paths_segment_intercept(int proj_index , CachedPathProfile &cache) {
    const int total_size = static_cast<int>(global_paths.size());
    // 定义前后点数,截取片段
    int num_prev = 6;
    // 计算可能的起始和结束索引
    int start_index = std::max(0, proj_index - num_prev);
    int end_index = std::min(total_size - 1, proj_index + num_next);
    // 如果前面的点不够，后面补
    if (proj_index - start_index < num_prev) {
        int extra_needed = num_prev - (proj_index - start_index);
        end_index = std::min(total_size - 1, end_index + extra_needed);
    }
    // 如果后面的点不够，前面补
    if (end_index - proj_index < num_next) {
        int extra_needed = num_next - (end_index - proj_index);
        start_index = std::max(0, start_index - extra_needed);
    }
    // 清空之前的片段
    PathSegment.CachedClear();
    // 提取子路径并记录车辆点索引
    for (int i = start_index; i <= end_index; ++i) {
        PathSegment.CachedAdd(global_paths[i], cache.headings[i], cache.accumulated_s[i],
            cache.kappas[i], cache.dkappas[i]);
    }
    PathSegment.index = proj_index;


    // auto CubicSpline2D_start = std::chrono::steady_clock::now();
    CubSpline2 = std::make_unique<CubicSpline2D>(PathSegment.path_segment, 
                                                                                PathSegment.accumulated_s, proj_index);
    /* auto CubicSpline2D_end = std::chrono::steady_clock::now();
    double CubicSpline2D_millsecond = std::chrono::duration<double, std::milli>(CubicSpline2D_end - CubicSpline2D_start).count();
    std::cout << CubicSpline2D_millsecond << "ms" << std::endl; */

    return !PathSegment.path_segment.empty();
}

void Paths_Routing::Scan(const sensor_msgs::LaserScan::ConstPtr& scan_msg){
    sensor_msgs::LaserScan scan_receive_ = *scan_msg;
    
    if(!global_paths.empty() && is_global_s_heading_)
    {
        obs.odomCallback(odom);
        obs.getGlobalPoints(global_paths, projection_point.index);
        obs.laserCallback(scan_receive_);
    }
}

void Paths_Routing::GoalPose(const geometry_msgs::PoseStamped::ConstPtr& goal_msg){
    if(is_goal_receive == false){
        goal_pose_x = goal_msg->pose.position.x;
        goal_pose_y = goal_msg->pose.position.y;
        goal_pose_yaw = tf2::getYaw(goal_msg->pose.orientation);
        is_goal_receive = true;
    }
}

void Paths_Routing::Odom(const nav_msgs::Odometry::ConstPtr& odom_msg){
    odom = *odom_msg;
    vehicle_outline();
}

void Paths_Routing::Global_Smooth_Paths(const nav_msgs::Path::ConstPtr& global_paths_msg){
    global_paths.clear();

    if(global_paths_msg->poses.size() > 0){
        for(size_t i = 0; i < global_paths_msg->poses.size(); i++){
            std::pair<double, double> global_point;
            global_point.first = global_paths_msg->poses[i].pose.position.x;
            global_point.second = global_paths_msg->poses[i].pose.position.y;
            global_paths.push_back(global_point);
        }
    }
}

void Paths_Routing::vehicle_outline(){
    double x = odom.pose.pose.position.x;
    double y = odom.pose.pose.position.y;
    double yaw = tf2::getYaw(odom.pose.pose.orientation);
    double half_width = prp.vehicle_width / 2;
    double half_length = prp.vehicle_length / 2;

    std::vector<geometry_msgs::Point> corners;

    for (auto &corners_point : std::vector<std::pair<double, double>>{
            {half_length, half_width},
            {half_length, - half_width},
            {- half_length, - half_width},
            {- half_length, half_width}}) {
        geometry_msgs::Point temporary_point;
        temporary_point.x = x + corners_point.first * cos(yaw) - corners_point.second * sin(yaw);
        temporary_point.y = y + corners_point.first * sin(yaw) + corners_point.second * cos(yaw);
        temporary_point.z = 0;
        corners.push_back(temporary_point);
    }
    // 连接最后一个点到第一个点，完成闭环
    corners.push_back(corners.front());

    // 创建Marker
    visualization_msgs::Marker marker_outline;
    marker_outline.header.frame_id = "map";
    marker_outline.header.stamp = ros::Time::now();
    marker_outline.ns = "vehicle";
    marker_outline.id = 0;
    marker_outline.type = visualization_msgs::Marker::LINE_STRIP;
    marker_outline.action = visualization_msgs::Marker::ADD;
    marker_outline.scale.x = 0.1; // 线条宽度

    // 车辆轮廓颜色
    marker_outline.color.r = 1.0;
    marker_outline.color.g = 0.0;
    marker_outline.color.b = 0.0;
    marker_outline.color.a = 1.0;
    // 添加顶点
    marker_outline.points = corners;

    // 发布Marker
    vehicle_outline_pub_.publish(marker_outline);
}

void Paths_Routing::joinThread(std::unique_ptr<std::thread>& th) {
    if (th && th->joinable()) {
        th->join();
    }
}

Paths_Routing::~Paths_Routing()
{
    joinThread(segment_intercept_thread_);
    joinThread(paths_routing_thread_);
}

FrenetHyperparameters Paths_Routing::createFrenetHyperparameters() 
{
    return {
        prp.Max_speed,                                       // 最大速度 [m/s]，纵向和横向速度的尺量合
        prp.Max_accel,                                        // 最大加速度[m/ss]
        prp.Max_curvature,                              // 最大曲率 [1/m]
        prp.left_d,                                                // 最大道路宽度 [m]
        prp.right_d,                                             // 最小道路宽度 [m]
        0.0,                                                                                     //道路中间宽度，正好为0
        prp.dDelta,                                              // 道路宽度采样间隔 [m]，值越小，轨迹好的越多，但是太小计算效率不行
        prp.tDelta,                                               // 时间采样间隔[s]，值越大生成的轨迹速度越快
        prp.MaxT,                                                 // 最大预测时间 [s]，值越大生成的轨迹距离越远
        prp.MinT,                                                 // 最小预测时间 [s]，值越大生成的轨迹长度越长
        prp.D_t_s,                                              // 目标速度采样间隔 [m/s]
        prp.N_s_sample,                                // 目标速度的采样数量
        prp.D_s,                                                 // 目标位置采样间隔 [m/s]
        prp.N_s_sample,                               // 目标位置的采样数量

        // 损失函数权重
        5.0, // Distance from reference path
        5.0, // Target speed
        1.0, // Target acceleration
        0.5, // Jerk
        0.5, // time
        // 总的
        5.0, // Lateral
        1, // Longitudin
    };
}

FrenetInitialConditions Paths_Routing::createFrenetInitialConditions() 
{
    return {
        init_lon_state,                                               // 初始的纵向值[m]
        ds0,                                                                    // 初始的纵向速度[m/s]
        dds0,                                                                 // 初始的纵向加速度[m/ss]
        d0,                                                                      // 初始的横向偏移值 [m]
        dd0,                                                                   // 初始的横向速度 [m/s]
        ddd0,                                                                // 初始的横向加速度 [m/s^2]
        0.0,                                                                    // 目标横向速度配置
        0.0,                                                                    // 目标横向加速度配置
        prp.target_speed,               // 目标速度（即纵向的速度保持） [m/s]
        20,                                                                     // 目标位置,就是前探距离，测试3，按理说要变量赋值
    };
}

bool Paths_Routing::is_collision(std::unique_ptr<FrenetPath>& fts, 
                const double& COLLISION_CHECK, const std::vector<Point>& obstacles)
{
    const size_t step_size = 3; 

    for (const auto& obstacle_ptr : obstacles) {
        // 初步过滤：使用宽松条件快速筛选

        for (size_t j = 0; j < fts->x.size(); j += step_size) {
            double front_x = fts->x[j];
            double front_y = fts->y[j];
            double yaw = fts->theta[j];
            // 计算前后两个点的位置
            double rear_x = front_x - prp.vehicle_length * cos(yaw);
            double rear_y = front_y - prp.vehicle_length * sin(yaw);

            // 检查前后点与障碍物的距离
            for (const auto& point : {std::make_pair(front_x, front_y), std::make_pair(rear_x, rear_y)}) 
            {
                double dx = obstacle_ptr.x - point.first;
                double dy = obstacle_ptr.y - point.second;
                double distance = sqrt(dx * dx + dy *dy);
                
                if (distance <= COLLISION_CHECK) {
                    return true;
                }
            }
        }
    }
    return false;
}
